/**
 * @file   QMiraP3ArmPainter.cpp
 * @author Josep Sabater (jsabaterm@el.ub.edu)
 * @date   November, 2016
 * @version 1.0.0
 * @ingroup QMira
 * @brief  Implementation of a P3 Arm painter
 *
 * Contains the methods implementation each P3Arm Painter must implement.
 */

#include <cassert>
#include <iostream>

#include "QMiraP3ArmPainter.h"
#include "MiraIRobot2D.h"

namespace QMira{

    enum JointLabel { J1 = 0, J2, J3, J4, J_TOTAL };    
    enum LinkLabel  { L1 = 0, L2, L3, L4, L_TOTAL };
    
    QP3ArmPainter::QP3ArmPainter(const IRobot2D &robot)
                        : robot_(robot)
                        , linksLen_(robot.getLinksLength()) { init_(); }    
    
    QP3ArmPainter::QP3ArmPainter(const IRobot2D &robot, const MiraUtilUBLib::DoubleMatrix_t &qs) 
                                    : robot_(robot)
                                    , linksLen_(robot.getLinksLength()){
        
        init_();
        for (auto q : qs)
            framesCont_.push_back(robot_.getJoints(q));              
    }
    
    QP3ArmPainter::~QP3ArmPainter() = default;    

    void QP3ArmPainter::draw(QPainter &painter, const QIRobot2DPainter::FrameId_t frameId) const {
        assert( !framesCont_.empty() );

        /* If there are no more frames to paint, repaint the last one */
        const unsigned int nElem = framesCont_.size();        
        QIRobot2DPainter::FrameId_t fr2Paint = (frameId < nElem) ? frameId : nElem-1;
        const Joint2D::Joint2DCont_t &frame = framesCont_[fr2Paint];

        assert(frame.size() == JointLabel::J_TOTAL);
        drawJointPath_(frame[JointLabel::J1], painter, Qt::blue, j1Path_);
        drawJoint_(frame[JointLabel::J2], painter, Qt::red);
        drawLongTubePath_(frame[JointLabel::J3], painter, Qt::blue, l3Path_);
        
        painter.save();
            QPen pen(Qt::gray);
            pen.setStyle(Qt::DotLine);
            painter.setPen(pen);
            QPointF j1Pos(frame[JointLabel::J1].x(), frame[JointLabel::J1].y());
            QPointF j2Pos(frame[JointLabel::J2].x(), frame[JointLabel::J2].y());
            painter.drawEllipse(j1Pos, linksLen_[LinkLabel::L1], linksLen_[LinkLabel::L1]);
            painter.drawEllipse(j2Pos, linksLen_[LinkLabel::L2], linksLen_[LinkLabel::L2]);
        painter.restore();
        
        drawJoint_(frame[JointLabel::J3], painter, Qt::green);
        drawJoint_(frame[JointLabel::J4], painter, Qt::yellow);

        drawLink_(frame[JointLabel::J1], frame[JointLabel::J3], painter, Qt::black);
        drawLink_(frame[JointLabel::J2], frame[JointLabel::J4], painter, Qt::black);
        drawLink_(frame[JointLabel::J3], frame[JointLabel::J4], linksLen_[LinkLabel::L3], painter, Qt::black);      
    }

    QIRobot2DPainter::FrameId_t QP3ArmPainter::addFrame(const MiraUtilUBLib::DoubleCont_t &q) {
        framesCont_.push_back(robot_.getJoints(q));
        return framesCont_.size()-1;
    }

    unsigned int QP3ArmPainter::clearFrames() {
        unsigned int removedFrames = framesCont_.size();
        framesCont_.clear();
        return removedFrames;
    }
    
    void QP3ArmPainter::init_() {
        /* Prepare Painter Path for J1 */
        const int radius = 10;
        j1Path_.moveTo(-radius, 0);
        j1Path_.lineTo(-radius, radius);
        j1Path_.lineTo(radius,radius);
        j1Path_.lineTo(radius, 0);
        j1Path_.arcTo(-radius,-radius,2*radius,2*radius,0,180);               
        
        /* Prepare Painter Path for L3 */
        const double longTubeRadiusMM = 25.4/2.;
        const double j3ToEndTipDistMM = 330.4032 + 3.2258;
        const double tipLenMM = 103.0478 - 30;
        l3Path_.moveTo(0, longTubeRadiusMM);
        l3Path_.lineTo(-j3ToEndTipDistMM+tipLenMM, longTubeRadiusMM);
        l3Path_.lineTo(-j3ToEndTipDistMM, 0);
        l3Path_.lineTo(-j3ToEndTipDistMM+tipLenMM, -longTubeRadiusMM);
        l3Path_.lineTo(0, -longTubeRadiusMM);
        l3Path_.closeSubpath();
        //l3Path_.lineTo(0, longTubeRadiusMM)        
    }
    
    void QP3ArmPainter::drawJoint_(const Joint2D &joint, QPainter &painter, const QColor &color) const {

        QPen pen;
        pen.setColor(color);
        pen.setWidth(5);

        painter.save();
            painter.setPen(pen);
            QPointF point(joint.x(), joint.y());
            painter.drawEllipse(point, 10, 10);
        painter.restore();   
    }
    
    void QP3ArmPainter::drawJointPath_(const Joint2D &joint, 
                                         QPainter &painter, 
                                         const QColor &color, 
                                         const QPainterPath &path) const {
        QPen pen;
        pen.setColor(color);
        pen.setWidth(5);

        painter.save();
            painter.translate(joint.x(), joint.y());
            painter.rotate(joint.location().orient);
            painter.setPen(pen);
            painter.drawPath(path);
        painter.restore();
    }

    void QP3ArmPainter::drawLongTubePath_(const Joint2D &joint, 
                                         QPainter &painter, 
                                         const QColor &color, 
                                         const QPainterPath &path) const {
        QPen pen;
        pen.setColor(color);
        pen.setWidth(5);

        painter.save();
            painter.translate(joint.x(), joint.y());
            painter.rotate(joint.location().orient);
            painter.setPen(pen);
            painter.drawPath(path);
        painter.restore();
    }

    void QP3ArmPainter::drawLink_(const Joint2D &jointA,
                                  const Joint2D &jointB,
                                  QPainter &painter, 
                                  const QColor &color) const {

        QLineF link;
        link.setP1( QPointF(jointA.x(), jointA.y()) );
        link.setP2( QPointF(jointB.x(), jointB.y()) );

        QPen pen;
        pen.setColor(color);
        pen.setWidth(3);

        painter.save();
            painter.setPen(pen);
            painter.drawLine(link);
        painter.restore();            
    }
    
    void QP3ArmPainter::drawLink_(  const Joint2D &jointA,
                                    const Joint2D &jointB,
                                    const unsigned int length,
                                    QPainter &painter, 
                                    const QColor &color) const {
        QLineF link;
        link.setP1( QPointF(jointA.x(), jointA.y()) );
        link.setP2( QPointF(jointB.x(), jointB.y()) );
        link.setLength(length);

        QPen pen;
        pen.setColor(color);
        pen.setWidth(3);

        painter.save();
            painter.setPen(pen);
            painter.drawLine(link);
        painter.restore();         
        
    }   
}